#!/usr/bin/python

import time, sys
from SimpleCV import *
from collections import deque
from quickhull2d import qhull
from numpy import *

def distance(a, b):
    x1, y1 = a
    x2, y2 = b
    return sqrt((x2 - x1)**2 + (y2 - y1)**2)

class sma():
    def __init__(self, period):
        assert period == int(period) and period > 0, "Period must be an integer >0"
        self.period = period
        self.stream = deque()
 
    def __call__(self, n):
        stream = self.stream
        stream.append(n)    # appends on the right
        streamlength = len(stream)
        if streamlength > self.period:
            stream.popleft()
            streamlength -= 1
        if streamlength == 0:
            average = 0
        else:
            average = sum( stream ) / streamlength
 
        return average

#setup the camera
cam = Camera()
draw_message = False
sys.setrecursionlimit(1000000)
moving_delay = 5.0 #seconds
last_blob_time = 0
positions = []
size_sma = sma(5)

PINK = (247,199,247)
PINK_COMPLEMENT = (0x08,0x38,0x08)

lastImg = cam.getImage().colorDistance(PINK_COMPLEMENT)
lastImg.show()

while(True):
    newImg = cam.getImage().colorDistance(PINK_COMPLEMENT)
    trackImg = newImg - lastImg # diff the images
    trackImg = trackImg.erode(iterations=2) # smooth the image
    blobs = trackImg.findBlobs(threshval=50, minsize=20)

    now = time.time()
    last_blob_time = now if last_blob_time == 0 else last_blob_time
    
    if blobs is None:
        if (now - last_blob_time) > moving_delay:
            print 'Pleo stopped moving?'
            positions = list(set(positions))
            posarray = array([[x,y] for (x,y) in positions])
            hull = qhull(posarray)
            hull = deque(hull)
            position = hull.popleft()
            while len(hull) > 0:
                lineto = hull.popleft()
                newImg.drawLine(position, lineto, color=(0,0,255), thickness=5)
                position = lineto
            lastImg = newImg # update the image
            newImg.show()
            break
        else:
            continue
    else:   
        largest_blob = blobs[-1]

        if len(positions) > 1:
            for i in range(len(positions) - 1):
                a = (positions[i][0], positions[i][1])
                b = (positions[i+1][0], positions[i+1][1])
                newImg.drawLine(a, b, color=(255, 0, 0), thickness=2)

        last_blob_time = now
        blobpos = (largest_blob.x, largest_blob.y)

        avg = size_sma(largest_blob.area())
        sizediff = abs(largest_blob.area() - avg)

        if sizediff < (avg/2):
            positions.append(blobpos)
            newImg.drawCircle(blobpos, 20, color=(255,0,0), thickness=5)
            
        
        lastImg = newImg # update the image
        newImg.show()
        #time.sleep(0.01)
